#include <assert.h>
#include <capstone/capstone.h>
#include <pthread.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <unistd.h>

#include "cpu.h"

enum {
    BANK_USR,
    BANK_SVC,
    BANK_ABT,
    BANK_UND,
    BANK_IRQ,
    BANK_FIQ,
};

const char err_spsr[] = "SPSR access in usr/sys mode";

const char *gpr_name[] = {
    "r00_usr",
    "r01_usr",
    "r02_usr",
    "r03_usr",
    "r04_usr",
    "r05_usr",
    "r06_usr",
    "r07_usr",
    "r08_usr",
    "r09_usr",
    "r10_usr",
    "r11_usr",
    "r12_usr",
    "r13_usr",
    "r14_usr",
    "r13_svc",
    "r14_svc",
    "r13_abt",
    "r14_abt",
    "r13_und",
    "r14_und",
    "r13_irq",
    "r14_irq",
    "r08_fiq",
    "r09_fiq",
    "r10_fiq",
    "r11_fiq",
    "r12_fiq",
    "r13_fiq",
    "r14_fiq",
};

const char *spsr_name[5] = {
    "spsr_svc",
    "spsr_abt",
    "spsr_und",
    "spsr_irq",
    "spsr_fiq",
};

struct nzcv {
    bool n, z, c, v;
};

struct shifter_in {
    uint32_t rm;
    uint8_t shift;
    bool c;
};

struct shifter_out {
    uint32_t sh;
    bool c;
};

/*
 *  0-12 usr  0-12
 * 13-14 usr 13-14
 * 15-16 svc 13-14
 * 17-18 abt 13-14
 * 19-20 und 13-14
 * 21-22 irq 13-14
 * 23-29 fiq  8-14
 */
uint32_t gpr[30];
uint32_t pc;
struct nzcv cpsr_nzcv;
uint8_t cpsr_mode;
uint8_t cpsr_bank; // derived from cpsr_mode
struct {
    bool a, i, f;
} cpsr_aif;
uint32_t spsr[5];

csh capstone;

uint32_t bus_in[3], bus_out[4];

// returns -1 if mode is undefined
int mode_to_bank(uint8_t mode)
{
    switch (mode) {
    case 16: // usr
        return BANK_USR;
    case 17: // fiq
        return BANK_FIQ;
    case 18: // irq
        return BANK_IRQ;
    case 19: // svc
        return BANK_SVC;
    case 23: // und
        return BANK_UND;
    case 31: // sys
        return BANK_USR;
    }
    return -1;
}

int translate_index(int index, int bank)
{
    assert(index >= 0 && index < 15);
    if (index < 8) {
        return index;
    }
    if (bank == BANK_FIQ) {
        return 15+index;
    } else {
        if (index < 13) {
            return index;
        }
        // index == 13 || index == 14
        return bank+bank+index;
    }
}

const char *get_gpr_name(int index, uint8_t mode)
{
    int bank = mode_to_bank(mode);
    int tindex = translate_index(index, bank);
    return gpr_name[tindex];
}

void quit(void)
{
#if 0
    uint32_t dout[4] = { 2, 0, 0, 0 };
    //printf("sending output\n");
    write(cpu_socket, dout, sizeof dout);
#endif
    bus_out[0] = 2;
    bus_out[1] = 0;
    bus_out[2] = 0;
    bus_out[3] = 0;
    sem_post(&mem_sem);
    pthread_exit(NULL);
}

uint32_t getgpr_bank(int index, int bank)
{
    if (index == 15) {
        return pc+4; // pc = address of next instruction
    }
    return gpr[translate_index(index, bank)];
}

uint32_t getgpr(int index)
{
    return getgpr_bank(index, cpsr_bank);
}

void setgpr_bank(int index, int bank, uint32_t data)
{
    if (index == 15) {
        pc = data;
    } else {
        int tindex = translate_index(index, bank);
        gpr[tindex] = data;
        printf("$%s=%08x\n", gpr_name[tindex], data);
    }
}

void setgpr(int index, uint32_t data)
{
    setgpr_bank(index, cpsr_bank, data);
}

#if 0
void format_bin(char *s, uint32_t x, int n)
{
    uint32_t mask = 1 << n-1;
    int i;
    for (i=0; i<n; i++) {
        s[i] = x&mask ? '1' : '0';
        mask >>= 1;
    }
    s[i] = 0;
}
#endif

#if 0

void log_psr_nzcv(const char *prefix, uint8_t nzcv)
{
    printf("%%%s_nzcv=%c%c%c%c\n", prefix,
           '0'|nzcv>>3&1, '0'|nzcv>>2&1, '0'|nzcv>>1&1, '0'|nzcv&1);
}

void log_psr_aif(const char *prefix, uint8_t aif)
{
    printf("%%%s_aif=%c%c%c\n", prefix,
           '0'|aif>>2&1, '0'|aif>>1&1, '0'|aif&1);
}

void log_psr_mode(const char *prefix, uint8_t mode)
{
    printf("%%%s_mode=%c%c%c%c%c\n", prefix,
           '0'|mode>>4, '0'|mode>>3&1, '0'|mode>>2&1, '0'|mode>>1&1, '0'|mode&1);
}

#endif

void setcpsr_nzcv(struct nzcv *nzcv)
{
    cpsr_nzcv = *nzcv;
    printf("%%cpsr_nzcv=%c%c%c%c\n",
           '0'|nzcv->n, '0'|nzcv->z, '0'|nzcv->c, '0'|nzcv->v);
}

void setcpsr_nzcv_packed(uint8_t nzcv)
{
    struct nzcv s;
    s.n = nzcv>>3&1;
    s.z = nzcv>>2&1;
    s.c = nzcv>>1&1;
    s.v = nzcv   &1;
    setcpsr_nzcv(&s);
}

void setcpsr_a(bool flag)
{
    cpsr_aif.a = flag;
    printf("%%cpsr_a=%c\n", flag ? '1' : '0');
}

void setcpsr_i(bool flag)
{
    cpsr_aif.i = flag;
    printf("%%cpsr_i=%c\n", flag ? '1' : '0');
}

void setcpsr_f(bool flag)
{
    cpsr_aif.f = flag;
    printf("%%cpsr_f=%c\n", flag ? '1' : '0');
}

void setcpsr_mode(uint8_t mode)
{
    mode &= 31;
    cpsr_mode = mode;
    cpsr_bank = mode_to_bank(mode);
    printf("%%cpsr_mode=%c%c%c%c%c\n",
           '0'|mode>>4, '0'|mode>>3&1, '0'|mode>>2&1, '0'|mode>>1&1, '0'|mode&1);
}

void setcpsr(uint32_t d)
{
    setcpsr_nzcv_packed(d >> 28  );
    setcpsr_a          (d >>  8&1);
    setcpsr_i          (d >>  7&1);
    setcpsr_f          (d >>  6&1);
    setcpsr_mode       (d        );
}

uint32_t getcpsr(void)
{
    uint32_t q = 0;
    if (cpsr_nzcv.n) q |= 1<<31;
    if (cpsr_nzcv.z) q |= 1<<30;
    if (cpsr_nzcv.c) q |= 1<<29;
    if (cpsr_nzcv.v) q |= 1<<28;
    if (cpsr_aif .a) q |= 1<< 8;
    if (cpsr_aif .i) q |= 1<< 7;
    if (cpsr_aif .f) q |= 1<< 6;
    q |= cpsr_mode;
    return q;
}

void setspsr(uint32_t d)
{
    int index = cpsr_bank-1;
    assert(index >= 0 && index < 5);
    spsr[index] = d;
    printf("%%%s=%08x\n", spsr_name[index], d);
}

uint32_t getspsr(void)
{
    int index = cpsr_bank-1;
    assert(index >= 0 && index < 5);
    return spsr[index];
}

// ignores addr[1:0]
uint32_t loadi(uint32_t addr)
{
#if 0
    uint32_t dout[4] = { 1, addr, 0, 0 };
    uint32_t din[3];
    do {
        //printf("sending output\n");
        write(cpu_socket, dout, sizeof dout);
        read(cpu_socket, din, sizeof din);
        //printf("input received\n");
    } while (!(din[0]&1));
    return din[1];
#endif
    bus_out[0] = 1;
    bus_out[1] = addr;
    bus_out[2] = 0;
    bus_out[3] = 0;
    do {
        sem_post(&mem_sem);
        sem_wait(&cpu_sem);
    } while (!(bus_in[0]&1));
    return bus_in[1];
}

// ignores addr[1:0]
uint32_t load(uint32_t addr, uint8_t sel)
{
#if 0
    uint32_t dout[4] = { (sel&15)<<4 | 4, 0, addr, 0 };
    uint32_t din[3];
    do {
        //printf("sending output\n");
        write(cpu_socket, dout, sizeof dout);
        read(cpu_socket, din, sizeof din);
        //printf("input received\n");
    } while (!(din[0]&2));
    return din[2];
#endif
    bus_out[0] = (sel&15)<<4 | 4;
    bus_out[1] = 0;
    bus_out[2] = addr;
    bus_out[3] = 0;
    do {
        sem_post(&mem_sem);
        sem_wait(&cpu_sem);
    } while (!(bus_in[0]&2));
    return bus_in[2];
}

// ignores addr[1:0]
void store(uint32_t addr, uint8_t sel, uint32_t data)
{
#if 0
    uint32_t dout[4] = { (sel&15)<<4 | 8, 0, addr, data };
    uint32_t din[3];
    do {
        //printf("sending output\n");
        write(cpu_socket, dout, sizeof dout);
        read(cpu_socket, din, sizeof din);
        //printf("input received\n");
    } while (!(din[0]&2));
#endif
    bus_out[0] = (sel&15)<<4 | 8;
    bus_out[1] = 0;
    bus_out[2] = addr;
    bus_out[3] = data;
    do {
        sem_post(&mem_sem);
        sem_wait(&cpu_sem);
    } while (!(bus_in[0]&2));

    addr &= ~3;
    if (sel   &1) printf("*%08x=%02x\n", addr  , data    &0xff);
    if (sel>>1&1) printf("*%08x=%02x\n", addr|1, data>> 8&0xff);
    if (sel>>2&1) printf("*%08x=%02x\n", addr|2, data>>16&0xff);
    if (sel>>3&1) printf("*%08x=%02x\n", addr|3, data>>24&0xff);
}

uint8_t condpass(uint8_t cond)
{
    uint8_t c;
    assert(cond<16);
    switch (cond>>1) {
    case 0: c = cpsr_nzcv.z; break;
    case 1: c = cpsr_nzcv.c; break;
    case 2: c = cpsr_nzcv.n; break;
    case 3: c = cpsr_nzcv.v; break;
    case 4: c = cpsr_nzcv.c && !cpsr_nzcv.z; break;
    case 5: c = cpsr_nzcv.n == cpsr_nzcv.v; break;
    case 6: c = !cpsr_nzcv.z && cpsr_nzcv.n == cpsr_nzcv.v; break;
    case 7: c = 1; break;
    default: __builtin_unreachable();
    }
    return c^(cond&1);
}

void lsl(struct shifter_in *in, struct shifter_out *out)
{
    if (in->shift == 0) {
        out->sh = in->rm;
        out->c = in->c;
    } else if (in->shift < 32) {
        out->sh = in->rm << in->shift;
        out->c = (in->rm>>32-in->shift)&1;
    } else if (in->shift == 32) {
        out->sh = 0;
        out->c = in->rm&1;
    } else /* shift > 32 */ {
        out->sh = 0;
        out->c = 0;
    }
}

void sr(uint8_t asr, struct shifter_in *in, struct shifter_out *out)
{
    if (in->shift == 0) {
        out->sh = in->rm;
        out->c = in->c;
    } else if (in->shift < 32) {
        out->sh = asr ? (int32_t)in->rm >> in->shift : in->rm >> in->shift;
        out->c = (in->rm>>in->shift-1)&1;
    } else if (in->shift == 32) {
        out->sh = 0;
        out->c = in->rm>>31;
    } else /* shift > 32 */ {
        out->sh = 0;
        out->c = 0;
    }
}

void ror(struct shifter_in *in, struct shifter_out *out)
{
    if (in->shift == 0) {
        out->sh = in->rm;
        out->c = in->c;
    } else if ((in->shift&31) == 0) {
        out->sh = in->rm;
        out->c = in->rm>>31;
    } else /* (shift&31) > 0 */ {
        in->shift = in->shift&31;
        out->sh = in->rm>>in->shift | in->rm<<32-in->shift;
        out->c = (in->rm>>in->shift-1)&1;
    }
}

uint32_t ror32(uint32_t x, uint8_t n)
{
    return x>>n | x<<32-n;
}

void decode_shifter_operand_reg(uint32_t instr, struct shifter_out *out)
{
    uint8_t rm_sel;
    uint8_t rs_sel;
    struct shifter_in in;
    rm_sel = instr&15;
    in.rm = getgpr(rm_sel);
    in.c = cpsr_nzcv.c;
    switch (instr>>4&7) {
    case 0: // LSL imm
        in.shift = instr>>7&31;
        lsl(&in, out);
        break;
    case 1: // LSL reg
        rs_sel = instr>>8&15;
        in.shift = getgpr(rs_sel)&255;
        lsl(&in, out);
        break;
    case 2: // LSR imm
    case 4: // ASR imm
        in.shift = instr>>7&31;
        if (!in.shift) {
            in.shift = 32;
        }
        sr(instr>>6&1, &in, out);
        break;
    case 3: // LSR reg
    case 5: // ASR reg
        rs_sel = instr>>8&15;
        in.shift = getgpr(rs_sel)&255;
        sr(instr>>6&1, &in, out);
        break;
    case 6: // ROR imm
        in.shift = instr>>7&31;
        if (in.shift) {
            ror(&in, out);
        } else {
            // RRX
            out->sh = cpsr_nzcv.c<<31 | in.rm>>1;
            out->c = in.rm&1;
        }
        break;
    case 7: // ROR reg
        rs_sel = instr>>8&15;
        in.shift = getgpr(rs_sel)&255;
        ror(&in, out);
        break;
    }
}

// does not touch n, z bits
uint32_t add(uint32_t a, uint32_t b, uint8_t c, struct nzcv *nzcv_out)
{
    uint64_t la = a;
    uint64_t lb = b;
    uint64_t lsum = la+lb+c;
    uint32_t sum = lsum;
    uint8_t cout = lsum>>32&1;
    nzcv_out->c = cout;
    nzcv_out->v = ((a>>31)^(b>>31)^(sum>>31)^cout)&1;
    return sum;
}

void unimplemented(void)
{
    fprintf(stderr, "sorry, instruction not implemented\n");
    quit();
}

void unpredictable(const char *msg)
{
    fprintf(stderr, "UNPREDICTABLE: msg\n");
    quit();
}

void load_byte_to_gpr(bool sign_ext, uint32_t addr, uint8_t rd_sel)
{
    uint8_t n = addr&3;
    uint32_t dat, dat_ext;
    dat = load(addr, 1<<n);
    dat_ext = dat >> 8*n;
    dat_ext = sign_ext ? (uint32_t)(int32_t)(int8_t)dat_ext : dat_ext & 0xff;
    setgpr(rd_sel, dat_ext);
}

void load_half_to_gpr(bool sign_ext, uint32_t addr, uint8_t rd_sel)
{
    uint8_t n = addr&3;
    uint32_t dat0, dat_ext;
    if (n == 3) {
        uint32_t dat1;
        dat0 = load(addr,   8);
        dat1 = load(addr+4, 1);
        dat_ext = dat1<<8 | dat0>>24;
    } else {
        dat0 = load(addr, 3<<n);
        dat_ext = dat0 >> 8*n;
    }
    dat_ext = sign_ext ? (uint32_t)(int32_t)(int16_t)dat_ext : dat_ext & 0xffff;
    setgpr(rd_sel, dat_ext);
}

void exec_dp(uint32_t instr)
{
    uint8_t opcode = instr>>21&15;
    uint8_t s_bit  = instr>>20&1;
    uint8_t rn_sel = instr>>16&15;
    uint8_t rd_sel = instr>>12&15;
    uint32_t rn = getgpr(rn_sel);
    uint32_t rd;
    uint32_t sh; // alias

    struct shifter_out shifter_out;
    struct nzcv nzcv;

    nzcv.v = cpsr_nzcv.v;

    // compute shifter operand and shifter_carry_out
    if (instr>>25&1) {
        // immediate
        uint8_t rot = instr>>7&30;
        uint32_t imm = instr&255;
        shifter_out.sh = imm>>rot | imm<<32-rot;
        shifter_out.c = rot ? shifter_out.sh>>31 : cpsr_nzcv.c;
    } else {
        decode_shifter_operand_reg(instr, &shifter_out);
    }

    nzcv.c = shifter_out.c;
    nzcv.v = cpsr_nzcv.v;
    sh = shifter_out.sh;

    switch (opcode) {
    case 0x0: rd =      rn & sh                     ; break; // AND
    case 0x1: rd =      rn ^ sh                     ; break; // EOR
    case 0x2: rd = add( rn, ~sh,           1, &nzcv); break; // SUB
    case 0x3: rd = add(~rn,  sh,           1, &nzcv); break; // RSB
    case 0x4: rd = add( rn,  sh,           0, &nzcv); break; // ADD
    case 0x5: rd = add( rn,  sh, cpsr_nzcv.c, &nzcv); break; // ADC
    case 0x6: rd = add( rn, ~sh, cpsr_nzcv.c, &nzcv); break; // SBC
    case 0x7: rd = add(~rn,  sh, cpsr_nzcv.c, &nzcv); break; // RSC
    case 0x8: rd =      rn & sh                     ; break; // TST
    case 0x9: rd =      rn ^ sh                     ; break; // TEQ
    case 0xa: rd = add( rn, ~sh,           1, &nzcv); break; // CMP
    case 0xb: rd = add( rn,  sh,           0, &nzcv); break; // CMN
    case 0xc: rd =      rn | sh                     ; break; // ORR
    case 0xd: rd =           sh                     ; break; // MOV
    case 0xe: rd =      rn &~sh                     ; break; // BIC
    case 0xf: rd =          ~sh                     ; break; // MVN
    default: __builtin_unreachable();
    }

    if ((opcode&12) != 8) {
        // exclude TST, TEQ, CMP, CMN
        setgpr(rd_sel, rd);
    }

    if (s_bit) {
        // update flags
        if (rd_sel == 15) {
            setcpsr(getspsr());
        } else {
            nzcv.n = rd >> 31;
            nzcv.z = rd == 0;
            setcpsr_nzcv(&nzcv);
        }
    }
}

void exec_ls(uint32_t instr)
{
    uint8_t rn_sel = instr>>16&15;
    uint8_t rd_sel = instr>>12&15;
    uint32_t rn = getgpr(rn_sel);
    uint32_t addr;
    uint32_t addr_writeback;
    uint32_t sh;
    uint8_t writeback = 0;
    struct shifter_out shifter_out;
    if (instr>>25&1) {
        // register offset
        decode_shifter_operand_reg(instr, &shifter_out);
        sh = shifter_out.sh;
    } else {
        // immediate offset
        sh = instr & 0xfff;
    }
    if (instr>>23&1) /* U bit */ {
        addr_writeback = rn + sh;
    } else {
        addr_writeback = rn - sh;
    }
    if (instr>>24&1) /* P bit */ {
        // offset and pre-indexed
        addr = addr_writeback;
        writeback = instr>>21&1;
    } else {
        // post-indexed
        addr = rn;
        writeback = 1;
    }
    if (instr>>20&1) {
        // load
        uint32_t dat0;
        if (instr>>22&1) /* B bit */ {
            load_byte_to_gpr(false, addr, rd_sel);
        } else {
            if (addr&3) {
                // unaligned
                uint8_t n = addr&3;
                uint8_t sel8 = 0xf<<n;
                uint32_t dat1;
                dat0 = load(addr  , sel8   );
                dat1 = load(addr+4, sel8>>4);
                setgpr(rd_sel, dat1<<32-8*n | dat0>>8*n);
            } else {
                dat0 = load(addr, 0xf);
                setgpr(rd_sel, dat0);
            }
        }
    } else {
        // store
        uint32_t rd = getgpr(rd_sel);
        if (instr>>22&1) /* B bit */ {
            uint8_t n = addr&3;
            store(addr, 1<<n, rd<<8*n);
        } else {
            if (addr&3) {
                // unaligned
                uint8_t n = addr&3;
                uint8_t sel8 = 0xf<<n;
                store(addr  , sel8   , rd<<   8*n);
                store(addr+4, sel8>>4, rd>>32-8*n);
            } else {
                store(addr, 0xf, rd);
            }
        }
    }
    if (writeback) {
        // base register writeback
        setgpr(rn_sel, addr_writeback);
    }
}

void exec_lsx(uint32_t instr)
{
    // L S H
    // 0 0 1 STRH
    // 0 1 0 LDRD
    // 0 1 1 STRD
    // 1 0 1 LDRH
    // 1 1 0 LDRSB
    // 1 1 1 LDRSH
    uint32_t rn;
    uint32_t of;
    uint32_t addr_writeback;
    uint32_t addr;
    uint8_t writeback;
    uint8_t rn_sel = instr>>16&15;
    uint8_t rd_sel = instr>>12&15;
    rn = getgpr(rn_sel);
    if (instr>>22&1) {
        // immediate offset
        of = instr>>4 & 0xf0 | instr & 0xf;
    } else {
        // register offset
        of = getgpr(instr&15);
    }
    if (instr>>23&1) /* U bit */ {
        addr_writeback = rn + of;
    } else {
        addr_writeback = rn - of;
    }
    if (instr>>24&1) /* P bit */ {
        // offset and pre-indexed
        addr = addr_writeback;
        writeback = instr>>21&1;
    } else {
        // post-indexed
        addr = rn;
        writeback = 1;
    }
    if (instr>>20&1) /* L bit */ {
        // L=1
        if (instr>>6&1) /* S bit */ {
            // S=1
            if (instr>>5&1) {
                // H=1 LDRSH
                load_half_to_gpr(true, addr, rd_sel);
            } else {
                // H=0 LDRSB
                load_byte_to_gpr(true, addr, rd_sel);
            }
        } else {
            // S=0 H=1 LDRH
            load_half_to_gpr(false, addr, rd_sel);
        }
    } else {
        // L=0
        if (instr>>6&1) /* S bit */ {
            // S=1
            if (instr>>5&1) {
                // H=1 STRD
                uint32_t rd0 = getgpr(rd_sel  );
                uint32_t rd1 = getgpr(rd_sel|1);
                store(addr  , 0xf, rd0);
                store(addr+4, 0xf, rd1);
            } else {
                // H=0 LDRD
                uint32_t rd0 = load(addr  , 0xf);
                uint32_t rd1 = load(addr+4, 0xf);
                setgpr(rd_sel  , rd0);
                setgpr(rd_sel|1, rd1);
            }
        } else {
            // S=0 H=1 STRH
            uint32_t rd = getgpr(rd_sel);
            uint8_t n = addr&3;
            if (n == 3) {
                store(addr  , 8, rd<<24);
                store(addr+4, 1, rd>> 8);
            } else {
                store(addr, 3<<n, rd<<8*n);
            }
        }
    }
    if (writeback) {
        // base register writeback
        setgpr(rn_sel, addr_writeback);
    }
}

void exec_lsm(uint32_t instr)
{
    uint8_t rn_sel = instr>>16&15;
#if 0
    if (rn_sel == 15) {
        unpredictable("LDM/STM with Rn=R15");
    }
#endif
    uint32_t rn = getgpr(rn_sel);
    uint32_t addr;
    uint32_t addr_writeback;
    uint8_t addr_mode = instr>>23&3;
    uint16_t reglist = (uint16_t) instr;
    uint8_t nreg = __builtin_popcount(reglist);
    switch (addr_mode) {
    case 0: // DA
        addr = rn - nreg*4 + 4;
        addr_writeback = rn - nreg*4;
        break;
    case 1: // IA
        addr = rn;
        addr_writeback = rn + nreg*4;
        break;
    case 2: // DB
        addr = rn - nreg*4;
        addr_writeback = rn - nreg*4;
        break;
    case 3: // IB
        addr = rn + 4;
        addr_writeback = rn + nreg*4;
        break;
    default: __builtin_unreachable();
    }
    while (nreg) {
        uint8_t rd_sel = __builtin_ctz(reglist);
        if (instr>>20&1) {
            // LDM
            uint32_t dat = load(addr, 0xf);
            uint8_t bank = instr>>22&1 ? BANK_USR : cpsr_bank;
            setgpr_bank(rd_sel, bank, dat);
        } else {
            // STM
            uint32_t rd;
            uint8_t bank = instr>>22&1 ? BANK_USR : cpsr_bank;
            rd = getgpr_bank(rd_sel, bank);
            store(addr, 0xf, rd);
        }
        reglist = reglist & reglist-1;
        addr += 4;
        nreg--;
    }
    if (instr>>21&1) {
        // base register writeback
        setgpr(rn_sel, addr_writeback);
    }
}

void exec_b(uint32_t instr)
{
    if (instr>>24&1) {
        setgpr(14, pc);
    }
    pc += 4 + ((int32_t)(instr<<8) >> 6);
}

void exec_bx(uint32_t instr)
{
    uint32_t rm = getgpr(instr&15);
    pc = rm & ~3;
}

void exec_cps(uint32_t instr)
{
    if (cpsr_mode & 3) {
        // in a privileged mode
        if (instr>>19&1) {
            bool flag = instr>>18&1;
            if (instr>>8&1) setcpsr_a(flag);
            if (instr>>7&1) setcpsr_i(flag);
            if (instr>>6&1) setcpsr_f(flag);
        }
        if (instr>>17&1) {
            setcpsr_mode(instr);
        }
    }
}

void exec_ext(uint32_t instr)
{
    // [22]=1 → zero-extend, [22]=0 → sign-extend
    // 1. ([21:20]=10) extract a byte from rm and extend it to 32 bits
    //    ([21:20]=00) extract 2 bytes from rm and extend them to 16 bits each
    //    ([   20]= 1) extract a halfword from rm and extend it to 32 bits
    // 2. ([19:16]!=1111) add result with rn
    //    ([19:16]==1111) do nothing
    // 3. store result into rd
    uint32_t rn, rm, rd;
    uint8_t rn_sel = instr>>16&15;
    uint8_t rot = instr>>7&24;
    rm = getgpr(instr&15);
    rd = ror32(rm, rot);
    if (instr>>20&1) {
        if (instr>>22&1) {
            // UXT(A)H
            rd &= 0xffff;
        } else {
            // SXT(A)H
            rd = (int32_t)(int16_t)rd;
        }
    } else {
        if (instr>>21&1) {
            if (instr>>22&1) {
                // UXT(A)B
                rd &= 0xff;
            } else {
                // SXT(A)B
                rd = (int32_t)(int8_t)rd;
            }
        } else {
            if (instr>>22&1) {
                // UXT(A)B16
                rd &= 0xff00ff;
            } else {
                // SXT(A)B16
                rd = ((uint32_t)(uint16_t)(int16_t)(int8_t)rd) | ((int32_t)(int8_t)(rd>>16))<<16;
            }
        }
    }
    if (rn_sel != 15) {
        rn = getgpr(rn_sel);
        if (instr>>21&1) {
            // *XTB, *XTH
            rd += rn;
        } else {
            // *XTAB16
            rd = ((rd+rn)&0xffff)|((rd&0xffff0000)+(rn&0xffff0000));
        }
    }
    setgpr(instr>>12&15, rd);
}

void exec_mul(uint32_t instr)
{
    uint32_t rd, rn, rs, rm;
    uint8_t rd_sel = instr>>16&15;
    uint8_t rn_sel = instr>>12&15;

    rs = getgpr(instr>>8&15);
    rm = getgpr(instr&15);

    if (instr>>23&1) {
        // 0000100xxxxxxxxxxxxx1001xxxx umull
        // 0000101xxxxxxxxxxxxx1001xxxx umlal
        // 0000110xxxxxxxxxxxxx1001xxxx smull
        // 0000111xxxxxxxxxxxxx1001xxxx smlal

        // Rd = (Rm * Rs)[63:32]
        // Rn = (Rm * Rs)[31: 0]
        // if S then begin
        //     N_Flag = Rd[31]
        //     Z_Flag = (Rd == 0) and (Rn == 0)
        // end

        uint64_t prod;

        if (instr>>22&1) {
            prod = (int64_t)(int32_t)rm * (int64_t)(int32_t)rs;
        } else {
            prod = (uint64_t)rm * (uint64_t)rs;
        }
        if (instr>>21&1) {
            uint64_t addend;
            rd = getgpr(rd_sel);
            rn = getgpr(rn_sel);
            addend = rd;
            addend = addend<<32 | rn;
            prod += addend;
        }
        rd = prod >> 32;
        rn = (uint32_t)prod;
        setgpr(rd_sel, rd);
        setgpr(rn_sel, rn);
        if (instr>>20&1) {
            // update flags
            struct nzcv nzcv;
            nzcv.n = rd >> 31;
            nzcv.z = prod == 0;
            nzcv.c = cpsr_nzcv.c;
            nzcv.v = cpsr_nzcv.v;
            setcpsr_nzcv(&nzcv);
        }
    } else {
        // 0000000xxxxx0000xxxx1001xxxx mul
        // 0000001xxxxx0000xxxx1001xxxx mla

        // Rd = if A then (Rm*Rs+Rn)[31:0] else (Rm*Rs)[31:0]
        // if S then begin
        //     N_Flag = Rd[31]
        //     Z_Flag = Rd == 0
        // end

        rd = rm * rs;
        if (instr>>21&1) {
            rn = getgpr(rn_sel);
            rd += rn;
        }
        setgpr(rd_sel, rd);
        if (instr>>20&1) {
            // update flags
            struct nzcv nzcv;
            nzcv.n = rd >> 31;
            nzcv.z = rd == 0;
            nzcv.c = cpsr_nzcv.c;
            nzcv.v = cpsr_nzcv.v;
            setcpsr_nzcv(&nzcv);
        }
    }
}

void exec_mrs(uint32_t instr)
{
    uint8_t rd_sel = instr>>12&15;
    uint32_t rd;
    if (instr>>22&1) {
        if (cpsr_bank == BANK_USR) {
            unpredictable(err_spsr);
        }
        rd = getspsr();
    } else {
        rd = getcpsr();
    }
    setgpr(rd_sel, rd);
}

void exec_msr(uint32_t instr)
{
    uint32_t d, q;
    uint32_t mask = 0;
    if (instr>>25&1) {
        uint8_t rot = instr>>7&30;
        uint32_t imm = instr&255;
        d = imm>>rot | imm<<32-rot;
    } else {
        d = getgpr(instr&15);
    }
    if (instr>>19&1) mask |= 0xff000000;
    if (instr>>18&1) mask |= 0x00ff0000;
    if (instr>>17&1) mask |= 0x0000ff00;
    if (instr>>16&1) mask |= 0x000000ff;
    if (instr>>22&1) {
        // set SPSR
        if (cpsr_bank == BANK_USR) {
            unpredictable(err_spsr);
        } else {
            q = getspsr();
            d = d & mask | q & ~mask;
            setspsr(d);
        }
    } else {
        // set CPSR
        q = getcpsr();
        d = d & mask | q & ~mask;
        if (instr>>19&1) setcpsr_nzcv_packed(d>>28);
        if (q&3) {
            // in a privileged mode
            if (instr>>17&1) {
                setcpsr_a(d>>8&1);
            }
            if (instr>>16&1) {
                setcpsr_i(d>>7&1);
                setcpsr_f(d>>6&1);
                setcpsr_mode(d);
            }
        }
    }
}

void exec_swi(uint32_t instr)
{
    uint32_t cpsr_saved = getcpsr();
    setcpsr_i(1);
    setcpsr_mode(19);
    setgpr(14, pc);
    setspsr(cpsr_saved);
    pc = 8;
}

void exec_movt(uint32_t instr)
{
    uint8_t rd_sel = instr>>12&15;
    uint32_t imm = (instr>>16&15)<<12 | instr&0xfff;
    uint32_t rd = getgpr(rd_sel);
    rd = imm<<16 | rd&0xffff;
    setgpr(rd_sel, rd);
}

void dispatch_uncond(uint32_t instr);
void dispatch_cond(uint32_t instr);

void execute_instruction(uint32_t instr)
{
    uint8_t cond = instr >> 28;
    if (cond == 0xf) {
        dispatch_uncond(instr);
    } else if (condpass(cond)) {
        dispatch_cond(instr);
    }
}

#if 0
static int bind_socket(void)
{
    int sockfd;
    socklen_t size;
    struct sockaddr_un client_addr, server_addr;

    sockfd = socket(AF_UNIX, SOCK_DGRAM, 0);
    if (sockfd < 0) {
        perror("socket");
        return -1;
    }
    memset(&client_addr, 0, sizeof client_addr);
    client_addr.sun_family = AF_UNIX;
    sprintf(client_addr.sun_path, "/tmp/armemu-cpu%u", getpid());
    size = offsetof(struct sockaddr_un, sun_path) + strlen(client_addr.sun_path);
    if (bind(sockfd, (struct sockaddr *)&client_addr, size) < 0) {
        perror("bind");
        return -1;
    }
    memset(&server_addr, 0, sizeof server_addr);
    server_addr.sun_family = AF_UNIX;
    sprintf(server_addr.sun_path, "/tmp/armemu-mem%u", getpid());
    size = offsetof(struct sockaddr_un, sun_path) + strlen(server_addr.sun_path);
    if (connect(sockfd, (struct sockaddr *) &server_addr, size) < 0) {
        perror("connect");
        return -1;
    }
    return sockfd;
}
#endif

void cpu_init(void)
{
#if 0
    cpu_socket = bind_socket();
    if (cpu_socket < 0) {
        abort();
    }
#endif

    cpsr_aif.a = true;
    cpsr_aif.i = true;
    cpsr_aif.f = true;
    cpsr_mode = 19;
    cpsr_bank = BANK_SVC;

    cs_open(CS_ARCH_ARM, CS_MODE_ARM, &capstone);
}

void *cpu_proc(void *_)
{
#if 0
    uint32_t din[3];
    read(cpu_socket, din, sizeof din);
#endif
    //printf("input received\n");
    sem_wait(&cpu_sem);

    for (;;) {
        uint32_t instr = loadi(pc);
        cs_insn *insn;
        cs_disasm(capstone, (uint8_t*)&instr, 4, pc, 1, &insn);
        if (insn) {
            printf("@%08x %08x  %-8s%s\n", pc, instr, insn->mnemonic, insn->op_str);
            cs_free(insn, 1);
        } else {
            printf("@%08x %08x  ???\n", pc, instr);
        }
        pc += 4;
        if (instr == 0xeafffffe) {
            quit();
        } else {
            execute_instruction(instr);
        }
    }

    return NULL;
}

// vim: sw=4 expandtab
